#include "usartx.h"
SEND_DATA Send_Data;
RECEIVE_DATA Receive_Data;
extern int Time_count;

void data_task(void *pvParameters)
{
	
   while(1)
    {	
			
			
			LOS_Msleep(50);
			
			
			data_transition(); 
			USART1_SEND();     
			USART3_SEND();     
			USART5_SEND();		 
			CAN_SEND();        
		}
}

void data_transition(void)
{
	Send_Data.Sensor_Str.Frame_Header = FRAME_HEADER_CAR; 
	Send_Data.Sensor_Str.Frame_Tail = FRAME_TAIL_CAR;     
	
	
	
	
	switch(Car_Mode)
	{	
		case Mec_Car:      
			Send_Data.Sensor_Str.X_speed = ((MOTOR_A.Encoder+MOTOR_B.Encoder+MOTOR_C.Encoder+MOTOR_D.Encoder)/4)*1000;
	    Send_Data.Sensor_Str.Y_speed = ((MOTOR_A.Encoder-MOTOR_B.Encoder+MOTOR_C.Encoder-MOTOR_D.Encoder)/4)*1000; 
	    Send_Data.Sensor_Str.Z_speed = ((-MOTOR_A.Encoder-MOTOR_B.Encoder+MOTOR_C.Encoder+MOTOR_D.Encoder)/4/(Axle_spacing+Wheel_spacing))*1000;         
		  break; 
		
		case FourWheel_Car:
      Send_Data.Sensor_Str.X_speed = ((MOTOR_A.Encoder+MOTOR_B.Encoder+MOTOR_C.Encoder+MOTOR_D.Encoder)/4)*1000; 
	    Send_Data.Sensor_Str.Y_speed = 0;
	    Send_Data.Sensor_Str.Z_speed = ((-MOTOR_B.Encoder-MOTOR_A.Encoder+MOTOR_C.Encoder+MOTOR_D.Encoder)/2/(Axle_spacing+Wheel_spacing))*1000;
		 break; 
		
		case Tank_Car:   
			Send_Data.Sensor_Str.X_speed = ((MOTOR_A.Encoder+MOTOR_B.Encoder)/2)*1000; 
			Send_Data.Sensor_Str.Y_speed = 0;
			Send_Data.Sensor_Str.Z_speed = ((MOTOR_B.Encoder-MOTOR_A.Encoder)/(Wheel_spacing)*1000);
			break; 
	}
	
	
	Send_Data.Sensor_Str.Accelerometer.X_data= accel[1]; 
	Send_Data.Sensor_Str.Accelerometer.Y_data=-accel[0]; 
	Send_Data.Sensor_Str.Accelerometer.Z_data= accel[2]; 
	
	
	Send_Data.Sensor_Str.Gyroscope.X_data= gyro[1]; 
	Send_Data.Sensor_Str.Gyroscope.Y_data=-gyro[0]; 
	if(Flag_Stop==0) 
		
	  
		Send_Data.Sensor_Str.Gyroscope.Z_data=gyro[2];  
	else  
		
    
		Send_Data.Sensor_Str.Gyroscope.Z_data=0;        
	
	
	
	Send_Data.Sensor_Str.Power_Voltage = Voltage*1000; 
	
	Send_Data.buffer[0]=Send_Data.Sensor_Str.Frame_Header; 
  Send_Data.buffer[1]=Flag_Stop; 
	
	
	
	Send_Data.buffer[2]=Send_Data.Sensor_Str.X_speed >>8; 
	Send_Data.buffer[3]=Send_Data.Sensor_Str.X_speed ;    
	Send_Data.buffer[4]=Send_Data.Sensor_Str.Y_speed>>8;  
	Send_Data.buffer[5]=Send_Data.Sensor_Str.Y_speed;     
	Send_Data.buffer[6]=Send_Data.Sensor_Str.Z_speed >>8; 
	Send_Data.buffer[7]=Send_Data.Sensor_Str.Z_speed ;    
	
	
	
	Send_Data.buffer[8]=Send_Data.Sensor_Str.Accelerometer.X_data>>8; 
	Send_Data.buffer[9]=Send_Data.Sensor_Str.Accelerometer.X_data;   
	Send_Data.buffer[10]=Send_Data.Sensor_Str.Accelerometer.Y_data>>8;
	Send_Data.buffer[11]=Send_Data.Sensor_Str.Accelerometer.Y_data;
	Send_Data.buffer[12]=Send_Data.Sensor_Str.Accelerometer.Z_data>>8;
	Send_Data.buffer[13]=Send_Data.Sensor_Str.Accelerometer.Z_data;
	
	
	
	Send_Data.buffer[14]=Send_Data.Sensor_Str.Gyroscope.X_data>>8;
	Send_Data.buffer[15]=Send_Data.Sensor_Str.Gyroscope.X_data;
	Send_Data.buffer[16]=Send_Data.Sensor_Str.Gyroscope.Y_data>>8;
	Send_Data.buffer[17]=Send_Data.Sensor_Str.Gyroscope.Y_data;
	Send_Data.buffer[18]=Send_Data.Sensor_Str.Gyroscope.Z_data>>8;
	Send_Data.buffer[19]=Send_Data.Sensor_Str.Gyroscope.Z_data;
	
	
	
	Send_Data.buffer[20]=Send_Data.Sensor_Str.Power_Voltage >>8; 
	Send_Data.buffer[21]=Send_Data.Sensor_Str.Power_Voltage; 

  
  
	Send_Data.buffer[22]=Check_Sum(22,1); 
	
	Send_Data.buffer[23]=Send_Data.Sensor_Str.Frame_Tail; 
}

void USART1_SEND(void)
{
  unsigned char i = 0;	
	
	for(i=0; i<24; i++)
	{
		usart1_send(Send_Data.buffer[i]);
	}	 
}

void USART3_SEND(void)
{
  unsigned char i = 0;	
	for(i=0; i<24; i++)
	{
		usart3_send(Send_Data.buffer[i]);
	}	 
}

void USART5_SEND(void)
{
  unsigned char i = 0;	
	for(i=0; i<24; i++)
	{
		usart5_send(Send_Data.buffer[i]);
	}	 
}

void CAN_SEND(void) 
{
	u8 CAN_SENT[8],i;
	
	for(i=0;i<8;i++)
	{
	  CAN_SENT[i]=Send_Data.buffer[i];
	}
	CAN1_Send_Num(0x101,CAN_SENT);
	
	for(i=0;i<8;i++)
	{
	  CAN_SENT[i]=Send_Data.buffer[i+8];
	}
	CAN1_Send_Num(0x102,CAN_SENT);
	
	for(i=0;i<8;i++)
	{
	  CAN_SENT[i]=Send_Data.buffer[i+16];
	}
	CAN1_Send_Num(0x103,CAN_SENT);
}

void uart1_init(u32 bound)
{  	 
	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;
	
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);	 
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); 

	GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1);	
	GPIO_PinAFConfig(GPIOA,GPIO_PinSource10 ,GPIO_AF_USART1);	 
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9|GPIO_Pin_10;
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            
	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       
	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            
	GPIO_Init(GPIOA, &GPIO_InitStructure);  		          
	
  
	NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
	
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=1 ;
	
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;		
	
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;	
  
	
	NVIC_Init(&NVIC_InitStructure);	
	
  
	USART_InitStructure.USART_BaudRate = bound; 
	USART_InitStructure.USART_WordLength = USART_WordLength_8b; 
	USART_InitStructure.USART_StopBits = USART_StopBits_1; 
	USART_InitStructure.USART_Parity = USART_Parity_No; 
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; 
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	
	USART_Init(USART1, &USART_InitStructure); 
	
	USART_ITConfig(USART1, USART_IT_RXNE, ENABLE); 
	USART_Cmd(USART1, ENABLE);                     
}

void uart2_init(u32 bound)
{  	 
	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;

	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);	 
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); 
	
	GPIO_PinAFConfig(GPIOD,GPIO_PinSource5,GPIO_AF_USART2);	
	GPIO_PinAFConfig(GPIOD,GPIO_PinSource6 ,GPIO_AF_USART2);	 
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5|GPIO_Pin_6;
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            
	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       
	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            
	GPIO_Init(GPIOD, &GPIO_InitStructure);  		          
	
	
	NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
	
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=1 ;
	
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;	
  
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  
	
	NVIC_Init(&NVIC_InitStructure);	
	
	
	USART_InitStructure.USART_BaudRate = bound; 
	USART_InitStructure.USART_WordLength = USART_WordLength_8b; 
	USART_InitStructure.USART_StopBits = USART_StopBits_1; 
	USART_InitStructure.USART_Parity = USART_Parity_No; 
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; 
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	
	USART_Init(USART2, &USART_InitStructure);      
	
	USART_ITConfig(USART2, USART_IT_RXNE, ENABLE); 
	USART_Cmd(USART2, ENABLE);                     
}

void uart3_init(u32 bound)
{  	 
  GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;
	
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);	 
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); 
	
	GPIO_PinAFConfig(GPIOD,GPIO_PinSource8,GPIO_AF_USART3);	
	GPIO_PinAFConfig(GPIOD,GPIO_PinSource9,GPIO_AF_USART3);	 
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_9;
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            
	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       
	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            
	GPIO_Init(GPIOD, &GPIO_InitStructure);  		          
	
  
  NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
	
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2 ;
	
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;		
	
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;	
  
	
	NVIC_Init(&NVIC_InitStructure);
	
  
	USART_InitStructure.USART_BaudRate = bound; 
	USART_InitStructure.USART_WordLength = USART_WordLength_8b; 
	USART_InitStructure.USART_StopBits = USART_StopBits_1; 
	USART_InitStructure.USART_Parity = USART_Parity_No; 
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; 
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	
  USART_Init(USART3, &USART_InitStructure);      
	
  USART_ITConfig(USART3, USART_IT_RXNE, ENABLE); 
  USART_Cmd(USART3, ENABLE);                     
}

void uart5_init(u32 bound)
{  	 
  GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;
	
	
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);	 
		
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);	 
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE); 

	GPIO_PinAFConfig(GPIOC,GPIO_PinSource12,GPIO_AF_UART5);	
	GPIO_PinAFConfig(GPIOD,GPIO_PinSource2 ,GPIO_AF_UART5);	 
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            
	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       
	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            
	GPIO_Init(GPIOC, &GPIO_InitStructure);  		          

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;            
	GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;          
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;       
	GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_UP;            
	GPIO_Init(GPIOD, &GPIO_InitStructure);  		          
	
  
  NVIC_InitStructure.NVIC_IRQChannel = UART5_IRQn;
	
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=2 ;
	
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;		
	
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;	
  
	
	NVIC_Init(&NVIC_InitStructure);
	
  
	USART_InitStructure.USART_BaudRate = bound; 
	USART_InitStructure.USART_WordLength = USART_WordLength_8b; 
	USART_InitStructure.USART_StopBits = USART_StopBits_1; 
	USART_InitStructure.USART_Parity = USART_Parity_No; 
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; 
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	
  USART_Init(UART5, &USART_InitStructure);      
  USART_ITConfig(UART5, USART_IT_RXNE, ENABLE); 
  USART_Cmd(UART5, ENABLE);                     
}

int USART1_IRQHandler(void)
{	
	if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) 
	{
		u8 Usart_Receive;
		static u8 Count_car,Count_moveit;
		static u8 rxbuf_car[11],rxbuf_moveit[16];
		int check=0,error=1,i;
		
		Usart_Receive = USART_ReceiveData(USART1); 
		if(Time_count<CONTROL_DELAY)
			
		  
			return 0;	
			
		
    rxbuf_car[Count_car]=Usart_Receive;  
		rxbuf_moveit[Count_moveit]=Usart_Receive;
		
		
    if(Usart_Receive == FRAME_HEADER_CAR||Count_car>0) 		Count_car++; else Count_car=0; 
		if(Usart_Receive == FRAME_HEADER_MOVEIT||Count_moveit>0) Count_moveit++; else Count_moveit=0; 
		
		if (Count_car== 11) 
		{   
				Count_car =0; 
				if(rxbuf_car[10] == FRAME_TAIL_CAR) 
				{			
					for(i=0; i<9; i++)
					{
						
						
						check=rxbuf_car[i]^check; 
					}
					if(check==rxbuf_car[9]) 
						
					  
					  error=0; 
					
					if(error==0)	 
				  {		
            if(Usart1_ON_Flag==0)
						{	
							
							
							Usart1_ON_Flag=1;
							Usart5_ON_Flag=0;
							APP_ON_Flag=0;
							PS2_ON_Flag=0;
							Remote_ON_Flag=0;
							CAN_ON_Flag=0;
							 
						}		
						command_lost_count=0;
						
						
						Move_X=(short)((rxbuf_car[3]<<8)+(rxbuf_car[4])); 
						Move_Y=(short)((rxbuf_car[5]<<8)+(rxbuf_car[6])); 
						Move_Z=(short)((rxbuf_car[7]<<8)+(rxbuf_car[8])); 
						
						
						
						Move_X=Move_X/1000; 
						Move_Y=Move_Y/1000; 
						Move_Z=Move_Z/1000; 
					}
			  }
		 }
		
		  if (Count_moveit == 11)	
		{   
				Count_moveit=0;
				if(rxbuf_moveit[10] == FRAME_TAIL_MOVEIT) 
				{
					 
					for(i=0; i<9; i++)
					{
						check=rxbuf_moveit[i]^check; 
					}
					if(check==rxbuf_moveit[9]) error=0; 
					
					if(error==0)	 
				  {		
						Moveit_Angle1=(short)((rxbuf_moveit[1]<<8)+(rxbuf_moveit[2])); 
						Moveit_Angle2=(short)((rxbuf_moveit[3]<<8)+(rxbuf_moveit[4])); 
						Moveit_Angle3=(short)((rxbuf_moveit[5]<<8)+(rxbuf_moveit[6])); 
						Moveit_Angle4=(short)((rxbuf_moveit[7]<<8)+(rxbuf_moveit[8])); 
						
						Moveit_Angle1=-Moveit_Angle1*0.001f; 
						Moveit_Angle2= Moveit_Angle2*0.001f; 
						Moveit_Angle3= Moveit_Angle3*0.001f; 
						Moveit_Angle4=-Moveit_Angle4*0.001f;
	
					}
			  }
		 }
	}
		return 0;	
}

int USART2_IRQHandler(void)
{	
	int Usart_Receive;
	if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) 
	{	      
		static u8 Flag_PID,i,j,Receive[50],Last_Usart_Receive;
		static float Data;
				
		Usart_Receive=USART2->DR; 
		
		if(Deviation_Count<CONTROL_DELAY)
			
		  
		  return 0;	

		if(Usart_Receive==0x41&&Last_Usart_Receive==0x41&&APP_ON_Flag==0)
			
		  
			
		  
				PS2_ON_Flag=0,Remote_ON_Flag=0,APP_ON_Flag=1,CAN_ON_Flag=0,Usart1_ON_Flag=0,Usart5_ON_Flag=0;
			  Last_Usart_Receive=Usart_Receive;	
		if(Usart_Receive==0x4B) 
			
		  
			Turn_Flag=1;  
	  else	if(Usart_Receive==0x49||Usart_Receive==0x4A) 
      
			
			Turn_Flag=0;	
			
		if(Turn_Flag==0) 
		{
			
			
			if(Usart_Receive>=0x41&&Usart_Receive<=0x48)  
			{	
				Flag_Direction=Usart_Receive-0x40;
			}
			else	if(Usart_Receive<=8)   
			{			
				Flag_Direction=Usart_Receive;
			}	
			else  Flag_Direction=0;
		}
		else if(Turn_Flag==1)
		{
			
			
			if     (Usart_Receive==0x43) Flag_Left=0,Flag_Right=1; 
			else if(Usart_Receive==0x47) Flag_Left=1,Flag_Right=0; 
			else                         Flag_Left=0,Flag_Right=0,Flag_Direction=0;
			if     (Usart_Receive==0x41) car_A_steer_flag=1;
			if     (Usart_Receive==0x45)	car_A_steer_flag=2;
			else  Flag_Direction=0;
		}
		if(Usart_Receive==0x58)  RC_Velocity=RC_Velocity+100; 
		if(Usart_Receive==0x59)  RC_Velocity=RC_Velocity-100; 
	  
		if(RC_Velocity<50)   RC_Velocity=50;
		else if(RC_Velocity>1200)  RC_Velocity=1200;
	 
	 
	 if(Usart_Receive==0x7B) Flag_PID=1;   
	 if(Usart_Receive==0x7D) Flag_PID=2;   

	 if(Flag_PID==1) 
	 {
		Receive[i]=Usart_Receive;
		i++;
	 }
	 if(Flag_PID==2) 
	 {
			if(Receive[3]==0x50) 	 PID_Send=1;
			else  if(Receive[1]!=0x23) 
      {								
				for(j=i;j>=4;j--)
				{
					Data+=(Receive[j-1]-48)*pow(10,i-j);
				}
				switch(Receive[1])
				 {
					 case 0x30:  Moveit_Angle1=Data*0.01f-1.57f;break; 
					 case 0x31:  Moveit_Angle2=Data*0.01f-1.57f;break; 
					 case 0x32:  Moveit_Angle3=Data*0.01f-1.57f;break;
					 case 0x33:  Moveit_Angle4=Data*0.01f-1.57f;break;
					 case 0x36:  RC_Velocity=Data;break;
					 case 0x37:  Velocity_KP=Data;break;
					 case 0x38:  Velocity_KI=Data;break;
				 }
      }		
      
      
			Flag_PID=0;
			i=0;
			j=0;
			Data=0;
			memset(Receive, 0, sizeof(u8)*50); 
	 }
   if(RC_Velocity<0)   RC_Velocity=0; 	 
  }
  return 0;	
}

int USART3_IRQHandler(void)
{	
	static u8 Count_car,Count_moveit;
	static u8 rxbuf_moveit[16];
	u8 Usart_Receive;
	int check=0,error=1,i;

	if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET) 
	{
		Usart_Receive = USART_ReceiveData(USART3);
		if(Time_count<CONTROL_DELAY)
			
		  
		  return 0;	
		
		
		
    Receive_Data.buffer[Count_car]=Usart_Receive;
		rxbuf_moveit[Count_moveit]=Usart_Receive;
		
		
		
		
		if(Usart_Receive == FRAME_HEADER_CAR||Count_car>0) Count_car++; else Count_car=0; 
		if(Usart_Receive == FRAME_HEADER_MOVEIT||Count_moveit>0) Count_moveit++; else Count_moveit=0; 
		if (Count_car == 11) 
		{   
				Count_car=0; 
				if(Receive_Data.buffer[10] == FRAME_TAIL_CAR) 
				{
					
					
					if(Receive_Data.buffer[9] ==Check_Sum(9,0))	 
				  {		
						
            
						PS2_ON_Flag=0;
						Remote_ON_Flag=0;
						APP_ON_Flag=0;
						CAN_ON_Flag=0;
						Usart1_ON_Flag=0;
						Usart5_ON_Flag=0;
						command_lost_count=0; 
						
						
						Move_X=XYZ_Target_Speed_transition(Receive_Data.buffer[3],Receive_Data.buffer[4]);
						Move_Y=XYZ_Target_Speed_transition(Receive_Data.buffer[5],Receive_Data.buffer[6]);
						Move_Z=XYZ_Target_Speed_transition(Receive_Data.buffer[7],Receive_Data.buffer[8]);
				  }
			}
		}
		  if (Count_moveit == 11)	
		{   
				Count_moveit=0;
				if(rxbuf_moveit[10] == FRAME_TAIL_MOVEIT) 
				{
					 
					for(i=0; i<9; i++)
					{
						check=rxbuf_moveit[i]^check; 
					}
					if(check==rxbuf_moveit[9]) error=0; 
					
					if(error==0)	 
				  {		
						Moveit_Angle1=(short)((rxbuf_moveit[1]<<8)+(rxbuf_moveit[2])); 
						Moveit_Angle2=(short)((rxbuf_moveit[3]<<8)+(rxbuf_moveit[4])); 
						Moveit_Angle3=(short)((rxbuf_moveit[5]<<8)+(rxbuf_moveit[6])); 
						Moveit_Angle4=(short)((rxbuf_moveit[7]<<8)+(rxbuf_moveit[8])); 
						
						Moveit_Angle1=-Moveit_Angle1*0.001f; 
						Moveit_Angle2= Moveit_Angle2*0.001f; 
						Moveit_Angle3= Moveit_Angle3*0.001f; 
						Moveit_Angle4=-Moveit_Angle4*0.001f;
	
					}
			  }
		 }
	} 
  return 0;	
}


int UART5_IRQHandler(void)
{	
	static u8 Count_car,Count_moveit;
	static u8 rxbuf_moveit[16];
	u8 Usart_Receive;
	int check=0,error=1,i;

	if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET) 
	{
		Usart_Receive = USART_ReceiveData(USART3);
		if(Time_count<CONTROL_DELAY)
			
		  
		  return 0;	
		
		
		
    Receive_Data.buffer[Count_car]=Usart_Receive;
		rxbuf_moveit[Count_moveit]=Usart_Receive;
		
		
		
		
		if(Usart_Receive == FRAME_HEADER_CAR||Count_car>0) Count_car++; else Count_car=0; 
		if(Usart_Receive == FRAME_HEADER_MOVEIT||Count_moveit>0) Count_moveit++; else Count_moveit=0; 
		if (Count_car == 11) 
		{   
				Count_car=0; 
				if(Receive_Data.buffer[10] == FRAME_TAIL_CAR) 
				{
					
					
					if(Receive_Data.buffer[9] ==Check_Sum(9,0))	 
				  {		
						
            
						PS2_ON_Flag=0;
						Remote_ON_Flag=0;
						APP_ON_Flag=0;
						CAN_ON_Flag=0;
						Usart1_ON_Flag=0;
						Usart5_ON_Flag=1;
						command_lost_count=0; 
						
						
						Move_X=XYZ_Target_Speed_transition(Receive_Data.buffer[3],Receive_Data.buffer[4]);
						Move_Y=XYZ_Target_Speed_transition(Receive_Data.buffer[5],Receive_Data.buffer[6]);
						Move_Z=XYZ_Target_Speed_transition(Receive_Data.buffer[7],Receive_Data.buffer[8]);
				  }
			}
		}
		  if (Count_moveit == 11)	
		{   
				Count_moveit=0;
				if(rxbuf_moveit[10] == FRAME_TAIL_MOVEIT) 
				{
					 
					for(i=0; i<9; i++)
					{
						check=rxbuf_moveit[i]^check; 
					}
					if(check==rxbuf_moveit[9]) error=0; 
					
					if(error==0)	 
				  {		
						Moveit_Angle1=(short)((rxbuf_moveit[1]<<8)+(rxbuf_moveit[2])); 
						Moveit_Angle2=(short)((rxbuf_moveit[3]<<8)+(rxbuf_moveit[4])); 
						Moveit_Angle3=(short)((rxbuf_moveit[5]<<8)+(rxbuf_moveit[6])); 
						Moveit_Angle4=(short)((rxbuf_moveit[7]<<8)+(rxbuf_moveit[8])); 
						
						Moveit_Angle1=-Moveit_Angle1*0.001f; 
						Moveit_Angle2= Moveit_Angle2*0.001f; 
						Moveit_Angle3= Moveit_Angle3*0.001f; 
						Moveit_Angle4=-Moveit_Angle4*0.001f;
	
					}
			  }
		 }
	} 
  return 0;	
}

float Vz_to_Akm_Angle(float Vx, float Vz)
{
	float R, AngleR, Min_Turn_Radius;
	
	
	
	
	
	
	
	
	Min_Turn_Radius=MINI_AKM_MIN_TURN_RADIUS;
	
	if(Vz!=0 && Vx!=0)
	{
		
		
		if(float_abs(Vx/Vz)<=Min_Turn_Radius)
		{
			
			
			if(Vz>0)
				Vz= float_abs(Vx)/(Min_Turn_Radius);
			else	
				Vz=-float_abs(Vx)/(Min_Turn_Radius);	
		}		
		R=Vx/Vz;
		
		AngleR=atan(Axle_spacing/(R+0.5f*Wheel_spacing));
	}
	else
	{
		AngleR=0;
	}
	
	return AngleR;
}

float XYZ_Target_Speed_transition(u8 High,u8 Low)
{
	
	
	short transition; 
	
	
	
	transition=((High<<8)+Low); 
	return 
		transition/1000+(transition%1000)*0.001; 
}

void usart1_send(u8 data)
{
	USART1->DR = data;
	while((USART1->SR&0x40)==0);	
}

void usart2_send(u8 data)
{
	USART2->DR = data;
	while((USART2->SR&0x40)==0);	
}

void usart3_send(u8 data)
{
	USART3->DR = data;
	while((USART3->SR&0x40)==0);	
}


void usart5_send(u8 data)
{
	UART5->DR = data;
	while((UART5->SR&0x40)==0);	
}

u8 Check_Sum(unsigned char Count_Number,unsigned char Mode)
{
	unsigned char check_sum=0,k;
	
	
	
	if(Mode==1)
	for(k=0;k<Count_Number;k++)
	{
	check_sum=check_sum^Send_Data.buffer[k];
	}
	
	
	
	if(Mode==0)
	for(k=0;k<Count_Number;k++)
	{
	check_sum=check_sum^Receive_Data.buffer[k];
	}
	return check_sum;
}






